#include "./PID_Control.hpp"

void PID_Control_Class::Init(float Kp, float Ki, float Kd)
{
    this->kp = Kp;
    this->ki = Ki;
    this->kd = Kd;

    pre_e = 0.0f;
    integral = 0.0f;
}

float PID_Control_Class::Cal(float e)
{
    /* 更新三个误差 */
    integral += e; /* sigma{e(i)} */

    float value;
    value = kp * e + ki * integral + kd * (e - pre_e);
    if (value < (decrease_limit))
    {
        value = decrease_limit;
    }
    else if (value > increase_limit)
    {
        value = increase_limit;
    }
    pre_e = e;

    return value;
}

void PID_Control_Class::Set_Limit(float increase_limit, float decrease_limit)
{
    this->increase_limit = (increase_limit > 0.0f ? increase_limit : -increase_limit);
    this->decrease_limit = (decrease_limit > 0.0f ? -decrease_limit : decrease_limit);
}
